#include "bucketcheck.h"

BucketCheck::BucketCheck(){
  nh_ = ros::NodeHandle();
  nh_private_ = ros::NodeHandle("~");
  laser_type_ = nh_private_.param("laser_type",3);
  target_bucket_radius_ = nh_private_.param("target_bucket_radius",50);
  target_circle_max_radius_ = nh_private_.param("target_circle_max_radius",50.2);
  target_circle_min_radius_ = nh_private_.param("target_circle_min_radius",49.8);
  rate_cluster_ = nh_private_.param("rate_cluster",0.7);

  if_bigBucket_select_ = nh_private_.param("if_bigBucket_select",false);
  select_bucket_radius_ = nh_private_.param("select_bucket_radius",50);
  select_circle_max_radius_ = nh_private_.param("select_circle_max_radius",50.2);
  select_circle_min_radius_ = nh_private_.param("select_circle_min_radius",49.8);

  sub_ = nh_.subscribe("/scan",20,&BucketCheck::laser_callback,this);  //订阅激光数据
};

void BucketCheck::laser_callback(const LaserScanConstPtr& laser){
       data_.clear();
       for(int i=0;i<laser->ranges.size();i++)
       {
         if(laser->ranges[i] > 0.1 && laser->ranges[i] < 6.0){
           data_.push_back(long(laser->ranges[i] * 1000));
         }else{
           data_.push_back(0);
         }

         stamp_ = laser->header.stamp;
         frame_id_ = laser->header.frame_id;
       }
}

void BucketCheck::zero_select(vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& final_lines,vector<float>& cluster_num){
  for(int i=0;i<cloud_clusters.size();)
  {
   int clear_num=0;
   for(int j=0;j<cloud_clusters[i]->points.size();j++)
   {
     if(cloud_clusters[i]->points[j].x!=376)
     {clear_num++;}
   }
   if(clear_num<10)
   {
     final_lines.erase(final_lines.begin()+i);
     cloud_clusters.erase(cloud_clusters.begin()+i);
     cluster_num.erase(cluster_num.begin()+i);
   }
   else
    i++;
  }
}

void BucketCheck::point_num_select(vector<long> data,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters,vector<float>& sum_average,vector<float>& cluster_num){
  for(int i=0;i<cloud_clusters.size();i++)
  {
    float sum_dis = 0;
    for(int j=0;j<cloud_clusters[i]->points.size();j++)
    {
       sum_dis = (float)sqrt((+cloud_clusters[i]->points[j].x-376)*10*(cloud_clusters[i]->points[j].x-376)*10
           +(600-cloud_clusters[i]->points[j].y)*10*(600-cloud_clusters[i]->points[j].y)*10) + sum_dis;
    }
    sum_average.push_back(sum_dis/cloud_clusters[i]->points.size());
  }

  for(int i=0;i<cloud_clusters.size();i++)
  {
     cluster_num.push_back(2*(atan(bucket_radius_*10/sum_average[i]))/((float)180/data.size()*3.14159/180));
  }

  //去除点数过小的类
  for(int i=0;i<cloud_clusters.size();)
  {
    //cout<<"rate::::"<<cloud_clusters[i]->points.size()/cluster_num[i]<<endl;
    if(cloud_clusters[i]->points.size()<rate_cluster_*cluster_num[i])
    {
      cloud_clusters.erase(cloud_clusters.begin()+i);
      cluster_num.erase(cluster_num.begin()+i);
      sum_average.erase(sum_average.begin()+i);
    }
    else
      ++i;
  }
}

void BucketCheck::point_num_select(vector<long> data,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters,vector<float>& sum_average,vector<float>& cluster_num,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& final_circles,vector<Eigen::VectorXf> &model_circles){
  for(int i=0;i<cloud_clusters.size();i++)
  {
    float sum_dis = 0;
    for(int j=0;j<cloud_clusters[i]->points.size();j++)
    {
       sum_dis = (float)sqrt((cloud_clusters[i]->points[j].x-376)*10*(cloud_clusters[i]->points[j].x-376)*10
           +(600-cloud_clusters[i]->points[j].y)*10*(600-cloud_clusters[i]->points[j].y)*10) + sum_dis;
    }
    sum_average.push_back(sum_dis/cloud_clusters[i]->points.size());
  }

  for(int i=0;i<cloud_clusters.size();i++)
  {
    //cout<<"dis::"<<sum_average[i];
    cluster_num.push_back(2*(atan(bucket_radius_*10/sum_average[i]))/((float)180/data.size()*3.14159/180));
  }

  //去除点数过小的类
  for(int i=0;i<cloud_clusters.size();)
  {
    if(cloud_clusters[i]->points.size()<rate_cluster_*cluster_num[i])
    {
      cloud_clusters.erase(cloud_clusters.begin()+i);
      cluster_num.erase(cluster_num.begin()+i);
      sum_average.erase(sum_average.begin()+i);
      model_circles.erase(model_circles.begin()+i);
    }
    else
      ++i;
  }
}

void BucketCheck::cluster_select(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters){
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud);
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (5); //cm
  ec.setMinClusterSize (10);
  ec.setMaxClusterSize (600);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud);
  ec.extract (cluster_indices);

  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
     for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
     cloud_cluster->points.push_back (cloud->points[*pit]); //*
     cloud_cluster->width = cloud_cluster->points.size ();
     cloud_cluster->height = 1;
     cloud_cluster->is_dense = true;
     cloud_clusters.push_back(cloud_cluster);
  }
}

void BucketCheck::line_select(vector<float>& sum_average,vector<float>& cluster_num,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& final_lines){
  for(int i=0;i<cloud_clusters.size();i++)
  {
    if(sum_average[i]>3000)
    {
       pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);   //存储提取的局内点
       std::vector<int> inliers;  //存储局内点集合的点的索引的向量
       pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr
       model_l (new pcl::SampleConsensusModelLine<pcl::PointXYZ> (cloud_clusters[i]));
       pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_l);
       ransac.setDistanceThreshold (3.5);    //与直线小于2的点称为局内点考虑
       ransac.computeModel();                   //执行随机参数估计
       ransac.getInliers(inliers);                 //存储估计所得的局内点
       pcl::copyPointCloud<pcl::PointXYZ>(*cloud_clusters[i], inliers, *final);
       final_lines.push_back(final);
    }
    else if(sum_average[i]<0&&sum_average[i]>900)
    {
       pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);   //存储提取的局内点
       std::vector<int> inliers;  //存储局内点集合的点的索引的向量
       pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr
       model_l (new pcl::SampleConsensusModelLine<pcl::PointXYZ> (cloud_clusters[i]));
       pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_l);
       ransac.setDistanceThreshold (2.5);    //与直线小于2的点称为局内点考虑
       ransac.computeModel();                   //执行随机参数估计
       ransac.getInliers(inliers);                 //存储估计所得的局内点
       pcl::copyPointCloud<pcl::PointXYZ>(*cloud_clusters[i], inliers, *final);
       final_lines.push_back(final);
    }
    else{
       pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);   //存储提取的局内点
       std::vector<int> inliers;  //存储局内点集合的点的索引的向量
       pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr
       model_l (new pcl::SampleConsensusModelLine<pcl::PointXYZ> (cloud_clusters[i]));
       pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_l);
       ransac.setDistanceThreshold (1);    //与直线小于2的点称为局内点考虑
       ransac.computeModel();                   //执行随机参数估计
       ransac.getInliers(inliers);                 //存储估计所得的局内点
       pcl::copyPointCloud<pcl::PointXYZ>(*cloud_clusters[i], inliers, *final);
       final_lines.push_back(final);
    }
  }
  ////局内点所占比率
  for(int i=0;i<cloud_clusters.size();)
  {
   if((float)final_lines[i]->points.size()/cloud_clusters[i]->points.size()>0.7)
   {
     final_lines.erase(final_lines.begin()+i);
     cloud_clusters.erase(cloud_clusters.begin()+i);
     cluster_num.erase(cluster_num.begin()+i);
   }
   else
   ++i;
  }
}

void BucketCheck::circle_select(vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& final_circles,vector<Eigen::VectorXf> &model_circles){
  for(int i=0;i<cloud_clusters.size();i++)
  {
    pcl::PointCloud<pcl::PointXYZ>::Ptr final_circle (new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<int> inliers_circle;
    Eigen::VectorXf model_coefficients;
    pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>::Ptr
    model_circle (new pcl::SampleConsensusModelCircle2D<pcl::PointXYZ> (cloud_clusters[i]));
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_circle);
    model_circle->radius_min_=circle_min_radius_;
    model_circle->radius_max_=circle_max_radius_;
    ransac.setDistanceThreshold (2);    //与平面距离小于2的点称为局内点考虑
    ransac.computeModel();                   //执行随机参数估计
    ransac.getModelCoefficients(model_coefficients);
    ransac.getInliers(inliers_circle);                 //存储估计所得的局内点
    pcl::copyPointCloud<pcl::PointXYZ>(*cloud_clusters[i], inliers_circle, *final_circle);
    final_circles.push_back(final_circle);
    model_circles.push_back(model_coefficients);
  }
}

void BucketCheck::cut_select(vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters,vector<float>& cluster_num,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& final_circles,vector<Eigen::VectorXf> &model_circles){
  for(int i=0;i<cloud_clusters.size();i++)
  {
    if(cloud_clusters[i]->points.size()>1.2*cluster_num[i])
    {
      vector<int> cut_left;
      int cloud_left=0;
      for(int j=0;j<cloud_clusters[i]->points.size();j++)
      {
        for(int m=0;m<final_circles[i]->points.size();m++)
        {
          if(cloud_clusters[i]->points[j].x==final_circles[i]->points[m].x&&cloud_clusters[i]->points[j].y==final_circles[i]->points[m].y)
          {
            cut_left.push_back(j);
            break;
          }
        }
      }

      for(int j=0;j<cut_left.size();j++)
      {
        if(cut_left[j]==cut_left[j+1]-1&&cut_left[j]==cut_left[j+2]-2&&cut_left[j]==cut_left[j+3]-3)
        {
          cloud_left=cut_left[j];
          break;
        }
      }

     if(cloud_left>0.6*cluster_num[i])
     {
       std::vector<int> inliers_circle_left;
       for(int j=0;j<cloud_left;j++)
       {
          inliers_circle_left.push_back(j);
       }
       pcl::PointCloud<pcl::PointXYZ>::Ptr final_circle_left (new pcl::PointCloud<pcl::PointXYZ>);
       pcl::copyPointCloud<pcl::PointXYZ>(*cloud_clusters[i], inliers_circle_left, *final_circle_left);
       cloud_clusters.push_back(final_circle_left);
       cluster_num.push_back(cluster_num[i]);
//    	cloud_clusters[i]->points.erase(cloud_clusters[i]->points.begin(),cloud_clusters[i]->points.begin()+cloud_left);
      ////圆拟合
       pcl::PointCloud<pcl::PointXYZ>::Ptr final_circle_l (new pcl::PointCloud<pcl::PointXYZ>);
       std::vector<int> inliers_circle_l ;
       Eigen::VectorXf model_coefficients_l;
       pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>::Ptr
       model_circle_l  (new pcl::SampleConsensusModelCircle2D<pcl::PointXYZ> (cloud_clusters[cloud_clusters.size()-1]));
       pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_circle_l );
       model_circle_l->radius_min_=circle_min_radius_;
       model_circle_l->radius_max_=circle_max_radius_;
       ransac.setDistanceThreshold (2);    //与平面距离小于2的点称为局内点考虑
       ransac.computeModel();                   //执行随机参数估计
       ransac.getModelCoefficients(model_coefficients_l);
       ransac.getInliers(inliers_circle_l );                 //存储估计所得的局内点
       pcl::copyPointCloud<pcl::PointXYZ>(*cloud_clusters[cloud_clusters.size()-1], inliers_circle_l , *final_circle_l );
       final_circles.push_back(final_circle_l);
       model_circles.push_back(model_coefficients_l);
     }

     vector<int> cut_right;
     int cloud_right=cloud_clusters[i]->points.size();
     for(int j=cloud_clusters[i]->points.size();j>0;j--)
     {
       for(int m=0;m<final_circles[i]->points.size();m++)
       {
          if(cloud_clusters[i]->points[j].x==final_circles[i]->points[m].x&&cloud_clusters[i]->points[j].y==final_circles[i]->points[m].y)
          {
            cut_right.push_back(j);
            break;
          }
       }
     }

     for(int j=0;j<cut_right.size();j++)
     {
       if(cut_right[j]==cut_right[j+1]+1)
       {
         cloud_right=cut_right[j];
         break;
       }
     }

     if(cloud_clusters[i]->points.size()-cloud_right>0.6*cluster_num[i])
     {
       std::vector<int> inliers_circle_right;
       for(int j=cloud_right;j<cloud_clusters[i]->points.size();j++)
       {
          inliers_circle_right.push_back(j);
       }
       pcl::PointCloud<pcl::PointXYZ>::Ptr final_circle_right (new pcl::PointCloud<pcl::PointXYZ>);
       pcl::copyPointCloud<pcl::PointXYZ>(*cloud_clusters[i], inliers_circle_right, *final_circle_right);
       cloud_clusters.push_back(final_circle_right);
       cluster_num.push_back(cluster_num[i]);
       ////圆拟合
       pcl::PointCloud<pcl::PointXYZ>::Ptr final_circle_r (new pcl::PointCloud<pcl::PointXYZ>);
       std::vector<int> inliers_circle_r ;
       Eigen::VectorXf model_coefficients_r;
       pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>::Ptr
       model_circle_r(new pcl::SampleConsensusModelCircle2D<pcl::PointXYZ> (cloud_clusters[cloud_clusters.size()-1]));
       pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_circle_r );
       model_circle_r->radius_min_=circle_min_radius_;
       model_circle_r->radius_max_=circle_max_radius_;
       ransac.setDistanceThreshold (2);    //与平面距离小于2的点称为局内点考虑
       ransac.computeModel();                   //执行随机参数估计
       ransac.getInliers(inliers_circle_r );                 //存储估计所得的局内点
       ransac.getModelCoefficients(model_coefficients_r);
       pcl::copyPointCloud<pcl::PointXYZ>(*cloud_clusters[cloud_clusters.size()-1], inliers_circle_r , *final_circle_r );
       final_circles.push_back(final_circle_r);
       model_circles.push_back(model_coefficients_r);
     }
     if(cloud_right<cloud_clusters[i]->points.size()&&cloud_clusters[i]->points.size()-cloud_right>0.4*cluster_num[i])
     cloud_clusters[i]->points.erase(cloud_clusters[i]->points.begin()+cloud_right,cloud_clusters[i]->points.end());
     if(cloud_left!=0&&cloud_left>0.4*cluster_num[i])
     cloud_clusters[i]->points.erase(cloud_clusters[i]->points.begin(),cloud_clusters[i]->points.begin()+cloud_left);

    }
  }

  for(int i=0;i<cloud_clusters.size();)
  {
     if((float)final_circles[i]->points.size()/cloud_clusters[i]->points.size()<0.7)
     {
      final_circles.erase(final_circles.begin()+i);
      cloud_clusters.erase(cloud_clusters.begin()+i);
      model_circles.erase(model_circles.begin()+i);
     }
     else
      ++i;
  }
}

void BucketCheck::teminal_select(vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& final_circles,vector<Eigen::VectorXf> &model_circles){
  //端点判别法
  for(int i=0;i<cloud_clusters.size();)
  {
   float cluster_left_x=cloud_clusters[i]->points[0].x;
   float cluster_left_y=cloud_clusters[i]->points[0].y;
   float cluster_right_x=cloud_clusters[i]->points[cloud_clusters[i]->points.size()-1].x;
   float cluster_right_y=cloud_clusters[i]->points[cloud_clusters[i]->points.size()-1].y;

   int clu_num=0;
   for(int j=1;j<cloud_clusters[i]->points.size()-2;j++)
   {
     if(((cluster_right_y-cluster_left_y)/(cluster_right_x-cluster_left_x)*(cloud_clusters[i]->points[j].x-cluster_left_x)+cluster_left_y-cloud_clusters[i]->points[j].y)<0)
     {clu_num++;}
   }
   //cout<<"bilv::"<<(float)clu_num/(cloud_clusters[i]->points.size()-2)<<endl;
   if((float)clu_num/(cloud_clusters[i]->points.size()-2)<0.7)
      {
        final_circles.erase(final_circles.begin()+i);
        cloud_clusters.erase(cloud_clusters.begin()+i);
        model_circles.erase(model_circles.begin()+i);
      }
      else
        ++i;
  }
}

void BucketCheck::slop_select(vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters,vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& final_circles,vector<Eigen::VectorXf> &model_circles){
  vector<float> sum_average;  //每一个聚类里面距离的平均值
  for(int i=0;i<cloud_clusters.size();i++)
  {
       float sum_dis = 0;
      for(int j=0;j<cloud_clusters[i]->points.size();j++)
      {
           sum_dis = (float)sqrt((cloud_clusters[i]->points[j].x-376)*10*(cloud_clusters[i]->points[j].x-376)*10
               +(600-cloud_clusters[i]->points[j].y)*10*(600-cloud_clusters[i]->points[j].y)*10) + sum_dis;
      }
      sum_average.push_back(sum_dis/cloud_clusters[i]->points.size());
  }

  for(int i=0;i<sum_average.size();){
    if(sum_average[i]>900){
        float left_x=(float)(cloud_clusters[i]->points[0].x+cloud_clusters[i]->points[1].x+cloud_clusters[i]->points[2].x)/3;
        float left_y=(float)(cloud_clusters[i]->points[0].y+cloud_clusters[i]->points[1].y+cloud_clusters[i]->points[2].y)/3;
        float k=0;
        vector<float> k_num;
        vector<float> k_average;
        int k_temp=0;
        for(int j=3;j<(int)cloud_clusters[i]->points.size()-3;j++)
        {
          k=(left_y-cloud_clusters[i]->points[j].y)/(left_x-cloud_clusters[i]->points[j].x);
          k_num.push_back(k);
        }
        for(int j=0;j<(int)k_num.size()-2;j=j+3)
        {
          k_average.push_back((k_num[j]+k_num[j+1]+k_num[j+2])/3);
        }
        for(int j=0;j<(int)k_average.size()-1;j++)
        {
          if(k_average[j+1]>k_average[j])
          k_temp++;
        }
        if((float)k_temp/((int)k_average.size()-1)<0.8)
           {
             final_circles.erase(final_circles.begin()+i);
             cloud_clusters.erase(cloud_clusters.begin()+i);
             model_circles.erase(model_circles.begin()+i);
             sum_average.erase(sum_average.begin()+i);
           }
           else
           ++i;
    }
    else{
      ++i;
    }
  }
}

void BucketCheck::bucketcheck(vector<long> data, vector<Eigen::VectorXf>& select_model_circles,vector<Eigen::VectorXf>& model_circles)
{
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);  //存储源点云
   cloud->width    = data.size();
   cloud->height   = 1;
   cloud->is_dense = true;

   cloud->points.resize (cloud->width * cloud->height);
   for(int j=0;j<data.size();++j)
   {
     float angle;
     if(laser_type_==1){
       //angle=3.14159*(((float)180/721)*(float)j)/180.0;
       angle=3.14159*(((float)180/721)*(float)j)/180.0+((1.5707963-1.207963)/1.5707963)*(3.14159/2);
     }
     else if(laser_type_==2){
       angle=3.14159*(((float)180/513)*(float)j)/180.0;
     }
     else if(laser_type_==3){
       angle=3.14159*(-24.6+0.00436*180/3.14*(float)j)/180.0;
     }
     float Xw=data[j]*cos(angle);
     float Zw=data[j]*sin(angle);
     int um=(int) (Xw/10+752/2);
     int vm=(int) (600-Zw/10);
     cloud->points[j].x=um;
     cloud->points[j].y=vm;
     cloud->points[j].z=0;
   }
	
   Mat Map2D_select(1000,752, CV_32FC3,Scalar(0,0,0));
   for(int i=0;i<cloud->points.size();i++)
   {
     Map2D_select.at<Vec3f>(cloud->points[i].y,cloud->points[i].x)[0]=255;
     Map2D_select.at<Vec3f>(cloud->points[i].y,cloud->points[i].x)[1]=255;
     Map2D_select.at<Vec3f>(cloud->points[i].y,cloud->points[i].x)[2]=255;
   }
   //imshow("111",Map2D_select);
   waitKey(10);

  if(if_bigBucket_select_ == true){

    bucket_radius_ = select_bucket_radius_;
    circle_max_radius_ = select_circle_max_radius_;
    circle_min_radius_ = select_circle_min_radius_;
    vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters;
    cluster_select(cloud,cloud_clusters);

    vector<float> sum_average;
    vector<float> cluster_num;
    point_num_select(data,cloud_clusters,sum_average,cluster_num);

    vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> final_lines;
    line_select(sum_average,cluster_num,cloud_clusters,final_lines);

    zero_select(cloud_clusters,final_lines,cluster_num);

    vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> final_circles;

    circle_select(cloud_clusters,final_circles,select_model_circles);

    cut_select(cloud_clusters,cluster_num,final_circles,select_model_circles);


    vector<float> sum_average_again;
    vector<float> cluster_num_again;
    point_num_select(data,cloud_clusters,sum_average_again,cluster_num_again,final_circles,select_model_circles);

    teminal_select(cloud_clusters,final_circles,select_model_circles);

  //  slop_select(cloud_clusters,final_circles,select_model_circles);

    Mat Map2D_(1000,752, CV_32FC3,Scalar(0,0,0));
    for(int i=0;i<cloud_clusters.size();i++)
    {
      for(int j=0;j<cloud_clusters[i]->points.size();j++)
      {
        Map2D_.at<Vec3f>(cloud_clusters[i]->points[j].y,cloud_clusters[i]->points[j].x)[0]=255;
        Map2D_.at<Vec3f>(cloud_clusters[i]->points[j].y,cloud_clusters[i]->points[j].x)[1]=255;
        Map2D_.at<Vec3f>(cloud_clusters[i]->points[j].y,cloud_clusters[i]->points[j].x)[2]=255;
      }
      circle(Map2D_, Point(select_model_circles[i][0],select_model_circles[i][1]),select_model_circles[i][2], CV_RGB(0, 255, 255), 1, 8, 0);
    }
    //imshow("big bucket check",Map2D_);
    waitKey(10);

    for(int i=0;i<cloud_clusters.size();i++)
    {
      for(int j=0;j<cloud_clusters[i]->points.size();j++)
      {
        for(int m=0;m<cloud->points.size();){
          if(cloud->points[m].x == cloud_clusters[i]->points[j].x&&
             cloud->points[m].y == cloud_clusters[i]->points[j].y){
            cloud->points.erase(cloud->points.begin()+m);
          }
          else{
            ++m;
          }
        }
      }
     }

  }

  model_circles.clear();

  bucket_radius_ = target_bucket_radius_;
  circle_max_radius_ = target_circle_max_radius_;
  circle_min_radius_ = target_circle_min_radius_;
  vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters;
  cluster_select(cloud,cloud_clusters);

  vector<float> sum_average;
  vector<float> cluster_num;
  point_num_select(data,cloud_clusters,sum_average,cluster_num);

  vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> final_lines;
  line_select(sum_average,cluster_num,cloud_clusters,final_lines);

  zero_select(cloud_clusters,final_lines,cluster_num);

  vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> final_circles;

  circle_select(cloud_clusters,final_circles,model_circles);

 cut_select(cloud_clusters,cluster_num,final_circles,model_circles);


  vector<float> sum_average_again;
  vector<float> cluster_num_again;
  point_num_select(data,cloud_clusters,sum_average_again,cluster_num_again,final_circles,model_circles);

  teminal_select(cloud_clusters,final_circles,model_circles);

  slop_select(cloud_clusters,final_circles,model_circles);


  Mat Map2D_select1(1000,752, CV_32FC3,Scalar(0,0,0));
  for(int i=0;i<cloud_clusters.size();i++)
  {
    for(int j=0;j<cloud_clusters[i]->points.size();j++)
    {
      Map2D_select1.at<Vec3f>(cloud_clusters[i]->points[j].y,cloud_clusters[i]->points[j].x)[0]=255;
      Map2D_select1.at<Vec3f>(cloud_clusters[i]->points[j].y,cloud_clusters[i]->points[j].x)[1]=255;
      Map2D_select1.at<Vec3f>(cloud_clusters[i]->points[j].y,cloud_clusters[i]->points[j].x)[2]=255;
    }
    circle(Map2D_select1, Point(model_circles[i][0],model_circles[i][1]),model_circles[i][2], CV_RGB(0, 255, 255), 1, 8, 0);
  }
  //imshow("",Map2D_select1);
  waitKey(10);
}



